<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN" "http://www.w3.org/TR/html4/loose.dtd">
<html lang="en">
<head>
<title>Source code</title>
<link rel="stylesheet" type="text/css" href="../../../../stylesheet.css" title="Style">
</head>
<body>
<div class="sourceContainer">
<pre><span class="sourceLineNo">001</span>package imagingbook.calibration.zhang;<a name="line.1"></a>
<span class="sourceLineNo">002</span><a name="line.2"></a>
<span class="sourceLineNo">003</span>import org.apache.commons.math3.linear.CholeskyDecomposition;<a name="line.3"></a>
<span class="sourceLineNo">004</span>import org.apache.commons.math3.linear.MatrixUtils;<a name="line.4"></a>
<span class="sourceLineNo">005</span>import org.apache.commons.math3.linear.RealMatrix;<a name="line.5"></a>
<span class="sourceLineNo">006</span><a name="line.6"></a>
<span class="sourceLineNo">007</span>import imagingbook.calibration.zhang.util.MathUtil;<a name="line.7"></a>
<span class="sourceLineNo">008</span>import imagingbook.lib.math.Matrix;<a name="line.8"></a>
<span class="sourceLineNo">009</span><a name="line.9"></a>
<span class="sourceLineNo">010</span>/**<a name="line.10"></a>
<span class="sourceLineNo">011</span> * This class defines methods for estimating the intrinsic camera parameters<a name="line.11"></a>
<span class="sourceLineNo">012</span> * from multiple homographies. Alternative versions are provided (only one is actually used though).<a name="line.12"></a>
<span class="sourceLineNo">013</span> * <a name="line.13"></a>
<span class="sourceLineNo">014</span> * @author W. Burger<a name="line.14"></a>
<span class="sourceLineNo">015</span> *<a name="line.15"></a>
<span class="sourceLineNo">016</span> */<a name="line.16"></a>
<span class="sourceLineNo">017</span>public class CameraIntrinsicsEstimator {<a name="line.17"></a>
<span class="sourceLineNo">018</span>        <a name="line.18"></a>
<span class="sourceLineNo">019</span><a name="line.19"></a>
<span class="sourceLineNo">020</span>        /**<a name="line.20"></a>
<span class="sourceLineNo">021</span>         * Version 1 (Zhang's original closed form solution).<a name="line.21"></a>
<span class="sourceLineNo">022</span>         * Estimates the the intrinsic camera parameters from multiple homographies.<a name="line.22"></a>
<span class="sourceLineNo">023</span>         * @param homographies a set of homography matrices<a name="line.23"></a>
<span class="sourceLineNo">024</span>         * @return the estimated 3 x 3 intrinsic transformation matrix<a name="line.24"></a>
<span class="sourceLineNo">025</span>         */<a name="line.25"></a>
<span class="sourceLineNo">026</span>        protected RealMatrix getCameraIntrinsicsZhang1(RealMatrix[] homographies) {<a name="line.26"></a>
<span class="sourceLineNo">027</span>                final int M = homographies.length;<a name="line.27"></a>
<span class="sourceLineNo">028</span>                int rows = 2 * M;<a name="line.28"></a>
<span class="sourceLineNo">029</span>                double[][] V = new double[rows][];<a name="line.29"></a>
<span class="sourceLineNo">030</span><a name="line.30"></a>
<span class="sourceLineNo">031</span>                for (int i = 0; i &lt; M; i++) {<a name="line.31"></a>
<span class="sourceLineNo">032</span>                        RealMatrix H = homographies[i];<a name="line.32"></a>
<span class="sourceLineNo">033</span>                        V[2*i] = getVpq(H, 0, 1); // v01<a name="line.33"></a>
<span class="sourceLineNo">034</span>                        V[2*i + 1] = Matrix.subtract(getVpq(H, 0, 0), getVpq(H, 1, 1)); // v00-v11<a name="line.34"></a>
<span class="sourceLineNo">035</span>                }<a name="line.35"></a>
<span class="sourceLineNo">036</span><a name="line.36"></a>
<span class="sourceLineNo">037</span>                if (M == 2) {<a name="line.37"></a>
<span class="sourceLineNo">038</span>                        V[V.length - 1] = new double[] { 0, 1, 0, 0, 0, 0 };<a name="line.38"></a>
<span class="sourceLineNo">039</span>                }<a name="line.39"></a>
<span class="sourceLineNo">040</span>                <a name="line.40"></a>
<span class="sourceLineNo">041</span>                RealMatrix VM = MatrixUtils.createRealMatrix(V);<a name="line.41"></a>
<span class="sourceLineNo">042</span>//              MathUtil.print("estimateIntrinsics: V = ", VM);//WB<a name="line.42"></a>
<span class="sourceLineNo">043</span>                <a name="line.43"></a>
<span class="sourceLineNo">044</span>                double[] b = MathUtil.solveHomogeneousSystem(VM).toArray();     // solve VM.b=0<a name="line.44"></a>
<span class="sourceLineNo">045</span>                <a name="line.45"></a>
<span class="sourceLineNo">046</span>                final double vc         = (b[1] * b[3] - b[0] * b[4]) / (b[0] * b[2] - b[1] * b[1]);<a name="line.46"></a>
<span class="sourceLineNo">047</span>                final double lambda = b[5] - (b[3] * b[3] + vc * (b[1] * b[3] - b[0] * b[4])) / b[0];<a name="line.47"></a>
<span class="sourceLineNo">048</span>                final double alpha      = Math.sqrt(lambda / b[0]);<a name="line.48"></a>
<span class="sourceLineNo">049</span>                final double beta       = Math.sqrt(lambda * b[0] / (b[0] * b[2] - b[1] * b[1]));<a name="line.49"></a>
<span class="sourceLineNo">050</span>                final double gamma      = -b[1] * alpha * alpha * beta / lambda;<a name="line.50"></a>
<span class="sourceLineNo">051</span>                final double uc         = gamma * vc / alpha - b[3] * alpha * alpha / lambda;   // PAMI paper = WRONG!<a name="line.51"></a>
<span class="sourceLineNo">052</span><a name="line.52"></a>
<span class="sourceLineNo">053</span>                RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {<a name="line.53"></a>
<span class="sourceLineNo">054</span>                                { alpha, gamma, uc },<a name="line.54"></a>
<span class="sourceLineNo">055</span>                                { 0, beta, vc },<a name="line.55"></a>
<span class="sourceLineNo">056</span>                                { 0, 0, 1 }<a name="line.56"></a>
<span class="sourceLineNo">057</span>                });<a name="line.57"></a>
<span class="sourceLineNo">058</span><a name="line.58"></a>
<span class="sourceLineNo">059</span>                return A;<a name="line.59"></a>
<span class="sourceLineNo">060</span>        }<a name="line.60"></a>
<span class="sourceLineNo">061</span>        <a name="line.61"></a>
<span class="sourceLineNo">062</span>        /**<a name="line.62"></a>
<span class="sourceLineNo">063</span>         * Version 2 (Zhang's corrected closed form solution).<a name="line.63"></a>
<span class="sourceLineNo">064</span>         * Estimates the the intrinsic camera parameters from multiple homographies.<a name="line.64"></a>
<span class="sourceLineNo">065</span>         * @param homographies a set of homography matrices<a name="line.65"></a>
<span class="sourceLineNo">066</span>         * @return the estimated 3 x 3 intrinsic transformation matrix<a name="line.66"></a>
<span class="sourceLineNo">067</span>         */<a name="line.67"></a>
<span class="sourceLineNo">068</span>        protected RealMatrix getCameraIntrinsicsZhang2(RealMatrix[] homographies) {<a name="line.68"></a>
<span class="sourceLineNo">069</span>                final int M = homographies.length;<a name="line.69"></a>
<span class="sourceLineNo">070</span>                int rows = 2 * M;<a name="line.70"></a>
<span class="sourceLineNo">071</span>                double[][] V = new double[rows][];<a name="line.71"></a>
<span class="sourceLineNo">072</span><a name="line.72"></a>
<span class="sourceLineNo">073</span>                for (int i = 0; i &lt; M; i++) {<a name="line.73"></a>
<span class="sourceLineNo">074</span>                        RealMatrix H = homographies[i];<a name="line.74"></a>
<span class="sourceLineNo">075</span>                        V[2*i] = getVpq(H, 0, 1); // v01<a name="line.75"></a>
<span class="sourceLineNo">076</span>                        V[2*i + 1] = Matrix.subtract(getVpq(H, 0, 0), getVpq(H, 1, 1)); // v00-v11<a name="line.76"></a>
<span class="sourceLineNo">077</span>                }<a name="line.77"></a>
<span class="sourceLineNo">078</span><a name="line.78"></a>
<span class="sourceLineNo">079</span>                if (M == 2) {<a name="line.79"></a>
<span class="sourceLineNo">080</span>                        V[V.length - 1] = new double[] { 0, 1, 0, 0, 0, 0 };<a name="line.80"></a>
<span class="sourceLineNo">081</span>                }<a name="line.81"></a>
<span class="sourceLineNo">082</span>                <a name="line.82"></a>
<span class="sourceLineNo">083</span>                RealMatrix VM = MatrixUtils.createRealMatrix(V);<a name="line.83"></a>
<span class="sourceLineNo">084</span>                double[] b = MathUtil.solveHomogeneousSystem(VM).toArray();     // solve VM.b=0<a name="line.84"></a>
<span class="sourceLineNo">085</span>                <a name="line.85"></a>
<span class="sourceLineNo">086</span>                final double vc         = (b[1] * b[3] - b[0] * b[4]) / (b[0] * b[2] - b[1] * b[1]);<a name="line.86"></a>
<span class="sourceLineNo">087</span>                final double lambda = b[5] - (b[3] * b[3] + vc * (b[1] * b[3] - b[0] * b[4])) / b[0];<a name="line.87"></a>
<span class="sourceLineNo">088</span>                final double alpha      = Math.sqrt(lambda / b[0]);<a name="line.88"></a>
<span class="sourceLineNo">089</span>                final double beta       = Math.sqrt(lambda * b[0] / (b[0] * b[2] - b[1] * b[1]));<a name="line.89"></a>
<span class="sourceLineNo">090</span>                final double gamma      = -b[1] * alpha * alpha * beta / lambda;<a name="line.90"></a>
<span class="sourceLineNo">091</span>                final double uc         = gamma * vc / beta - b[3] * alpha * alpha / lambda;    // beta! 1998 report seems correct!<a name="line.91"></a>
<span class="sourceLineNo">092</span><a name="line.92"></a>
<span class="sourceLineNo">093</span>                RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {<a name="line.93"></a>
<span class="sourceLineNo">094</span>                                { alpha, gamma, uc },<a name="line.94"></a>
<span class="sourceLineNo">095</span>                                { 0, beta, vc },<a name="line.95"></a>
<span class="sourceLineNo">096</span>                                { 0, 0, 1 }<a name="line.96"></a>
<span class="sourceLineNo">097</span>                });<a name="line.97"></a>
<span class="sourceLineNo">098</span><a name="line.98"></a>
<span class="sourceLineNo">099</span>                return A;<a name="line.99"></a>
<span class="sourceLineNo">100</span>        }<a name="line.100"></a>
<span class="sourceLineNo">101</span>        <a name="line.101"></a>
<span class="sourceLineNo">102</span>        /**<a name="line.102"></a>
<span class="sourceLineNo">103</span>         * Version 3 (WB's closed form solution).<a name="line.103"></a>
<span class="sourceLineNo">104</span>         * Estimates the the intrinsic camera parameters from multiple homographies.<a name="line.104"></a>
<span class="sourceLineNo">105</span>         * @param homographies a set of homography matrices<a name="line.105"></a>
<span class="sourceLineNo">106</span>         * @return the estimated 3 x 3 intrinsic transformation matrix<a name="line.106"></a>
<span class="sourceLineNo">107</span>         */<a name="line.107"></a>
<span class="sourceLineNo">108</span>        protected RealMatrix getCameraIntrinsicsZhang3(RealMatrix[] homographies) {<a name="line.108"></a>
<span class="sourceLineNo">109</span>                final int M = homographies.length;<a name="line.109"></a>
<span class="sourceLineNo">110</span>                int rows = 2 * M;<a name="line.110"></a>
<span class="sourceLineNo">111</span>                double[][] V = new double[rows][];<a name="line.111"></a>
<span class="sourceLineNo">112</span><a name="line.112"></a>
<span class="sourceLineNo">113</span>                for (int i = 0; i &lt; M; i++) {<a name="line.113"></a>
<span class="sourceLineNo">114</span>                        RealMatrix H = homographies[i];<a name="line.114"></a>
<span class="sourceLineNo">115</span>                        V[2*i] = getVpq(H, 0, 1); // v01<a name="line.115"></a>
<span class="sourceLineNo">116</span>                        V[2*i + 1] = Matrix.subtract(getVpq(H, 0, 0), getVpq(H, 1, 1)); // v00-v11<a name="line.116"></a>
<span class="sourceLineNo">117</span>                }<a name="line.117"></a>
<span class="sourceLineNo">118</span><a name="line.118"></a>
<span class="sourceLineNo">119</span>                if (M == 2) {<a name="line.119"></a>
<span class="sourceLineNo">120</span>                        V[V.length - 1] = new double[] { 0, 1, 0, 0, 0, 0 };<a name="line.120"></a>
<span class="sourceLineNo">121</span>                }<a name="line.121"></a>
<span class="sourceLineNo">122</span>                <a name="line.122"></a>
<span class="sourceLineNo">123</span>                RealMatrix VM = MatrixUtils.createRealMatrix(V);<a name="line.123"></a>
<span class="sourceLineNo">124</span>                double[] b = MathUtil.solveHomogeneousSystem(VM).toArray();     // solve VM.b=0<a name="line.124"></a>
<span class="sourceLineNo">125</span>                <a name="line.125"></a>
<span class="sourceLineNo">126</span>                final double w = b[0]*b[2]*b[5] - b[1]*b[1]*b[5] - b[0]*b[4]*b[4] + 2*b[1]*b[3]*b[4] - b[2]*b[3]*b[3];<a name="line.126"></a>
<span class="sourceLineNo">127</span>                final double d = <a name="line.127"></a>
<span class="sourceLineNo">128</span>                                b[0] * b[2] - b[1] * b[1];<a name="line.128"></a>
<span class="sourceLineNo">129</span>                final double uc = <a name="line.129"></a>
<span class="sourceLineNo">130</span>                                (b[1] * b[4] - b[2] * b[3]) / d;<a name="line.130"></a>
<span class="sourceLineNo">131</span>                final double vc = <a name="line.131"></a>
<span class="sourceLineNo">132</span>                                (b[1] * b[3] - b[0] * b[4]) / d;<a name="line.132"></a>
<span class="sourceLineNo">133</span>                final double alpha      = <a name="line.133"></a>
<span class="sourceLineNo">134</span>                                Math.sqrt(w / (d * b[0]));<a name="line.134"></a>
<span class="sourceLineNo">135</span>                final double beta       = <a name="line.135"></a>
<span class="sourceLineNo">136</span>                                Math.sqrt(w / (d * d) * b[0]);<a name="line.136"></a>
<span class="sourceLineNo">137</span>                final double gamma      = <a name="line.137"></a>
<span class="sourceLineNo">138</span>                                  Math.sqrt(w / (d * d * b[0])) * b[1];<a name="line.138"></a>
<span class="sourceLineNo">139</span>                <a name="line.139"></a>
<span class="sourceLineNo">140</span>                RealMatrix A = MatrixUtils.createRealMatrix(new double[][] {<a name="line.140"></a>
<span class="sourceLineNo">141</span>                                { alpha, gamma, uc },<a name="line.141"></a>
<span class="sourceLineNo">142</span>                                { 0, beta, vc },<a name="line.142"></a>
<span class="sourceLineNo">143</span>                                { 0, 0, 1 }<a name="line.143"></a>
<span class="sourceLineNo">144</span>                });<a name="line.144"></a>
<span class="sourceLineNo">145</span><a name="line.145"></a>
<span class="sourceLineNo">146</span>                return A;<a name="line.146"></a>
<span class="sourceLineNo">147</span>        }<a name="line.147"></a>
<span class="sourceLineNo">148</span>        <a name="line.148"></a>
<span class="sourceLineNo">149</span><a name="line.149"></a>
<span class="sourceLineNo">150</span>        /**<a name="line.150"></a>
<span class="sourceLineNo">151</span>         * Final version by WB (this version is used by default).<a name="line.151"></a>
<span class="sourceLineNo">152</span>         * Estimates the the intrinsic camera parameters from multiple homographies<a name="line.152"></a>
<span class="sourceLineNo">153</span>         * using a Cholesky decomposition.<a name="line.153"></a>
<span class="sourceLineNo">154</span>         * @param homographies a set of homography matrices<a name="line.154"></a>
<span class="sourceLineNo">155</span>         * @return the estimated 3 x 3 intrinsic transformation matrix<a name="line.155"></a>
<span class="sourceLineNo">156</span>         */<a name="line.156"></a>
<span class="sourceLineNo">157</span>        protected RealMatrix getCameraIntrinsics(RealMatrix[] homographies) {<a name="line.157"></a>
<span class="sourceLineNo">158</span>                final int M = homographies.length;<a name="line.158"></a>
<span class="sourceLineNo">159</span>                int rows = 2 * M;<a name="line.159"></a>
<span class="sourceLineNo">160</span>                double[][] V = new double[rows][];<a name="line.160"></a>
<span class="sourceLineNo">161</span><a name="line.161"></a>
<span class="sourceLineNo">162</span>                for (int i = 0; i &lt; M; i++) {<a name="line.162"></a>
<span class="sourceLineNo">163</span>                        RealMatrix H = homographies[i];<a name="line.163"></a>
<span class="sourceLineNo">164</span>                        V[2*i] = getVpq(H, 0, 1); // v01<a name="line.164"></a>
<span class="sourceLineNo">165</span>                        V[2*i + 1] = Matrix.subtract(getVpq(H, 0, 0), getVpq(H, 1, 1)); // v00-v11<a name="line.165"></a>
<span class="sourceLineNo">166</span>                }<a name="line.166"></a>
<span class="sourceLineNo">167</span><a name="line.167"></a>
<span class="sourceLineNo">168</span>                if (M == 2) {<a name="line.168"></a>
<span class="sourceLineNo">169</span>                        V[V.length - 1] = new double[] { 0, 1, 0, 0, 0, 0 };<a name="line.169"></a>
<span class="sourceLineNo">170</span>                }<a name="line.170"></a>
<span class="sourceLineNo">171</span>                <a name="line.171"></a>
<span class="sourceLineNo">172</span>                RealMatrix VM = MatrixUtils.createRealMatrix(V);<a name="line.172"></a>
<span class="sourceLineNo">173</span>//              MathUtil.print("estimateIntrinsics: V = ", VM);//WB<a name="line.173"></a>
<span class="sourceLineNo">174</span>                <a name="line.174"></a>
<span class="sourceLineNo">175</span>                double[] b = MathUtil.solveHomogeneousSystem(VM).toArray();     // solve VM.b=0<a name="line.175"></a>
<span class="sourceLineNo">176</span>                <a name="line.176"></a>
<span class="sourceLineNo">177</span>                RealMatrix B = MatrixUtils.createRealMatrix(new double[][]<a name="line.177"></a>
<span class="sourceLineNo">178</span>                                {{b[0], b[1], b[3]},<a name="line.178"></a>
<span class="sourceLineNo">179</span>                                 {b[1], b[2], b[4]},<a name="line.179"></a>
<span class="sourceLineNo">180</span>                                 {b[3], b[4], b[5]}});<a name="line.180"></a>
<span class="sourceLineNo">181</span>                <a name="line.181"></a>
<span class="sourceLineNo">182</span>                if (B.getEntry(0, 0) &lt; 0 || B.getEntry(1, 1) &lt; 0 || B.getEntry(2, 2) &lt; 0) {     <a name="line.182"></a>
<span class="sourceLineNo">183</span>                        B = B.scalarMultiply(-1);       // make sure B is positive definite <a name="line.183"></a>
<span class="sourceLineNo">184</span>                }<a name="line.184"></a>
<span class="sourceLineNo">185</span>                <a name="line.185"></a>
<span class="sourceLineNo">186</span>                CholeskyDecomposition cd = new CholeskyDecomposition(B);<a name="line.186"></a>
<span class="sourceLineNo">187</span>                RealMatrix L = cd.getL();<a name="line.187"></a>
<span class="sourceLineNo">188</span>                RealMatrix A = MatrixUtils.inverse(L).transpose().scalarMultiply(L.getEntry(2, 2));<a name="line.188"></a>
<span class="sourceLineNo">189</span>                return A;<a name="line.189"></a>
<span class="sourceLineNo">190</span>        }<a name="line.190"></a>
<span class="sourceLineNo">191</span><a name="line.191"></a>
<span class="sourceLineNo">192</span>//      private double[] getVpq(RealMatrix H, int p, int q) {<a name="line.192"></a>
<span class="sourceLineNo">193</span>//              H = H.transpose();<a name="line.193"></a>
<span class="sourceLineNo">194</span>//              final double[] vij = new double[] {<a name="line.194"></a>
<span class="sourceLineNo">195</span>//                              H.getEntry(p, 0) * H.getEntry(q, 0),<a name="line.195"></a>
<span class="sourceLineNo">196</span>//                              H.getEntry(p, 0) * H.getEntry(q, 1) + H.getEntry(p, 1) * H.getEntry(q, 0),<a name="line.196"></a>
<span class="sourceLineNo">197</span>//                              H.getEntry(p, 1) * H.getEntry(q, 1),<a name="line.197"></a>
<span class="sourceLineNo">198</span>//                              H.getEntry(p, 2) * H.getEntry(q, 0) + H.getEntry(p, 0) * H.getEntry(q, 2),<a name="line.198"></a>
<span class="sourceLineNo">199</span>//                              H.getEntry(p, 2) * H.getEntry(q, 1) + H.getEntry(p, 1) * H.getEntry(q, 2),<a name="line.199"></a>
<span class="sourceLineNo">200</span>//                              H.getEntry(p, 2) * H.getEntry(q, 2)<a name="line.200"></a>
<span class="sourceLineNo">201</span>//              };<a name="line.201"></a>
<span class="sourceLineNo">202</span>//              return vij;<a name="line.202"></a>
<span class="sourceLineNo">203</span>//      }<a name="line.203"></a>
<span class="sourceLineNo">204</span>        <a name="line.204"></a>
<span class="sourceLineNo">205</span>        // version without transpose<a name="line.205"></a>
<span class="sourceLineNo">206</span>        private double[] getVpq(RealMatrix H, int p, int q) {<a name="line.206"></a>
<span class="sourceLineNo">207</span>                final double[] vpq = new double[] {<a name="line.207"></a>
<span class="sourceLineNo">208</span>                                H.getEntry(0, p) * H.getEntry(0, q),<a name="line.208"></a>
<span class="sourceLineNo">209</span>                                H.getEntry(0, p) * H.getEntry(1, q) + H.getEntry(1, p) * H.getEntry(0, q),<a name="line.209"></a>
<span class="sourceLineNo">210</span>                                H.getEntry(1, p) * H.getEntry(1, q),<a name="line.210"></a>
<span class="sourceLineNo">211</span>                                H.getEntry(2, p) * H.getEntry(0, q) + H.getEntry(0, p) * H.getEntry(2, q),<a name="line.211"></a>
<span class="sourceLineNo">212</span>                                H.getEntry(2, p) * H.getEntry(1, q) + H.getEntry(1, p) * H.getEntry(2, q),<a name="line.212"></a>
<span class="sourceLineNo">213</span>                                H.getEntry(2, p) * H.getEntry(2, q)<a name="line.213"></a>
<span class="sourceLineNo">214</span>                };<a name="line.214"></a>
<span class="sourceLineNo">215</span>                return vpq;<a name="line.215"></a>
<span class="sourceLineNo">216</span>        }<a name="line.216"></a>
<span class="sourceLineNo">217</span><a name="line.217"></a>
<span class="sourceLineNo">218</span>}<a name="line.218"></a>




























































</pre>
</div>
</body>
</html>
